﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Kinect;
using Microsoft.DirectX;
using Microsoft.DirectX.Direct3D;

namespace Final_Project
{
    public class CTsdfAlg
    {
        private int m_nRows;
        private int m_nColumns;
        private float m_fFront = 400F;
        private float m_fBack = 4000F;

        public CTsdfAlg()
        {
            m_nRows = Convert.ToInt32(CXmlConfigReader.Instance().ReadValueFromElement("DepthMapResolutionRows"));
            m_nColumns = Convert.ToInt32(CXmlConfigReader.Instance().ReadValueFromElement("DepthMapResolutionColumns"));
        }

        public void AddMapToModel(ref DepthImagePixel[] cCurDepthPixelData, ref Microsoft.DirectX.Vector4[,] ViMatrix, ref float[,,] Model, ref Matrix PerspectiveMatrix, ref Matrix TransformMatrix)
        {
            Microsoft.DirectX.Vector4 A = new Microsoft.DirectX.Vector4();
            Microsoft.DirectX.Vector4 B = new Microsoft.DirectX.Vector4();
            Microsoft.DirectX.Vector4 C = new Microsoft.DirectX.Vector4();
            Microsoft.DirectX.Vector4 D = new Microsoft.DirectX.Vector4();
            Microsoft.DirectX.Vector4 E = new Microsoft.DirectX.Vector4();
            Matrix InvertedTransform = TransformMatrix;
            double x,y,Num = 0;

            A.W = 1;
            InvertedTransform.Invert();
            for(int i = 0; i < 512; i++)
            {
                A.X = (float)(0.5 * i);
                for(int j = 0; j < 512; j++)
                {
                    A.Y = (float)(0.5 * j);
                    for(int k = 0; k < 512; k++)
                    {
                        A.Z = (float)(0.5 * k);
                        C.X = A.X;
                        C.Y = A.Y;
                        C.Z = A.Z;
                        // Remove transform i
                        A.X = InvertedTransform.M11 * A.X + InvertedTransform.M12 * A.Y + InvertedTransform.M13 * A.Z + InvertedTransform.M14 * A.W;
                        A.Y = InvertedTransform.M21 * A.X + InvertedTransform.M22 * A.Y + InvertedTransform.M23 * A.Z + InvertedTransform.M24 * A.W;
                        A.Z = InvertedTransform.M31 * A.X + InvertedTransform.M32 * A.Y + InvertedTransform.M33 * A.Z + InvertedTransform.M34 * A.W;
                        // project A
                        B.X = (PerspectiveMatrix.M11 * A.X / A.Z + PerspectiveMatrix.M13);
                        B.Y = (PerspectiveMatrix.M22 * A.Y / A.Z + PerspectiveMatrix.M23);
                        // check if in frustum
                        if (B.X > 0 && B.X < m_nColumns && B.Y > 0 && B.Y < m_nRows)
                        {
                            x = Math.Ceiling(B.X);
                            y = Math.Ceiling(B.Y);
                            x--;
                            y--;
                            if (ViMatrix[(int)x, (int)y].Z > 0)
                            {
                                D.X = ViMatrix[(int)x, (int)y].X - TransformMatrix.M14;
                                D.Y = ViMatrix[(int)x, (int)y].Y - TransformMatrix.M24;
                                D.Z = ViMatrix[(int)x, (int)y].Z - TransformMatrix.M34;
                                E.X = C.X - TransformMatrix.M14;
                                E.Y = C.Y - TransformMatrix.M24;
                                E.Z = C.Z - TransformMatrix.M34;
                                Model[i, j, k] = D.Length() - E.Length();
                                /*float xl = (B.X - PerspectiveMatrix.M13) / PerspectiveMatrix.M11;
                                float yl = (B.Y - PerspectiveMatrix.M23) / PerspectiveMatrix.M22;
                                float lambda_inv = (float)Math.Sqrt(xl * xl + yl * yl + 1);
                                D.X = TransformMatrix.M14 - C.X; 
                                D.Y = TransformMatrix.M24 - C.Y;
                                D.Z = TransformMatrix.M34 - C.Z;
                                float Normal = (float)Math.Sqrt(D.X * D.X + D.Y * D.Y + D.Z * D.Z);
                                Model[i, j, k] = 1000 * Normal * lambda_inv - ViMatrix[(int)x, (int)y].Z * 1000; //mm
                                 * */
                                if (Model[i, j, k] < 1 && Model[i, j, k] > -1)
                                {
                                    Num++;
                                }
                            }
                            /*
                            if (cCurDepthPixelData[(int)x * m_nRows + (int)y].Depth > 0)
                            {
                                D.X = ViMatrix[(int)x, (int)y].X - TransformMatrix.M14;
                                D.Y = ViMatrix[(int)x, (int)y].Y - TransformMatrix.M24;
                                D.Z = ViMatrix[(int)x, (int)y].Z - TransformMatrix.M34;
                                E.X = i - TransformMatrix.M14;
                                E.Y = j - TransformMatrix.M24;
                                E.Z = k - TransformMatrix.M34;
                                Model[i, j, k] = D.Length() - E.Length();
                                if (Math.Abs(D.Length() - E.Length()) < 1)
                                {
                                    Num++;
                                }
                            }
                             */
                            
                        }
                    } // for(int k = 0; k < 512; k++)
                } // for(int j = 0; j < 512; j++)
            } // for(int i = 0; i < 512; i++)
        } // AddMapToModel

        public void Add(ref DepthImagePixel[] cCurDepthPixelData, ref Microsoft.DirectX.Vector4[,] ViMatrix, ref float[, ,] Model, ref Matrix PerspectiveMatrix, ref Matrix TransformMatrix)
        {
            int x, y, z;
            Matrix InvertedTransform = TransformMatrix;

            InvertedTransform.Invert();
            for (int i = 0; i < m_nColumns; i++)
            {
                for (int j = 0; j < m_nRows; j++)
                {
                    if (ViMatrix[i, j].Z > 0)
                    {
                        ViMatrix[i, j].X = InvertedTransform.M11 * ViMatrix[i, j].X + InvertedTransform.M12 * ViMatrix[i, j].Y + InvertedTransform.M13 * ViMatrix[i, j].Z + InvertedTransform.M14 * ViMatrix[i, j].W;
                        ViMatrix[i, j].Y = InvertedTransform.M21 * ViMatrix[i, j].X + InvertedTransform.M22 * ViMatrix[i, j].Y + InvertedTransform.M23 * ViMatrix[i, j].Z + InvertedTransform.M24 * ViMatrix[i, j].W;
                        ViMatrix[i, j].Z = InvertedTransform.M31 * ViMatrix[i, j].X + InvertedTransform.M32 * ViMatrix[i, j].Y + InvertedTransform.M33 * ViMatrix[i, j].Z + InvertedTransform.M34 * ViMatrix[i, j].W;
                        x = (int)(ViMatrix[i, j].X + 256);
                        y = (int)(ViMatrix[i, j].Y + 256);
                        z = (int)(ViMatrix[i, j].Z / 100 + 256);
                        if (x < 512 && y < 512 && z < 512)
                        {
                            Model[x, y, z] = 1;
                        }
                    }
                }
            }
            InvertedTransform.Invert();
        }
    }
}
